import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from nav_msgs.msg import Path
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseStamped
import math

class Go2OdomPath(Node):
    def __init__(self):
        super().__init__("odom2path_node_py")
        self.poses = list()  # 存储路径中途经点的列表
        self.last_pose = Pose()  # 列表中的最后一个途经点
        self.append_last_pose()  # 初始化操作，将原点添加进列表
        
        # 创建订阅方，订阅里程计消息
        self.odom_sub = self.create_subscription(Odometry, "odom", self.odom_cb, 10)
        # 创建发布方，发布轨迹信息   
        self.path_pub = self.create_publisher(Path, "path", 10)

    # 将点添加进列表
    def append_last_pose(self):
        # 组织PoseStamped对象
        ps = PoseStamped()
        ps.header.stamp = self.get_clock().now().to_msg()
        ps.header.frame_id = "odom"
        ps.pose = self.last_pose
        # 添加进列表
        self.poses.append(ps)

    # 获取当前机器人位姿与最后一个途径点的直线距离
    def get_distance(self, current_pose):
        dx = current_pose.position.x - self.last_pose.position.x 
        dy = current_pose.position.y - self.last_pose.position.y 
        return math.sqrt(math.pow(dx, 2) + math.pow(dy, 2))

    # 发布路径信息
    def publish_path(self):
        # 组织 Path 数据
        path_msg = Path()
        path_msg.header.stamp = self.get_clock().now().to_msg()
        path_msg.header.frame_id = "odom"
        path_msg.poses = self.poses
        # 发布
        self.path_pub.publish(path_msg)

    def odom_cb(self, odom):
        # 获取当前位姿
        current_pose = odom.pose.pose
        # 判断距离
        distance = self.get_distance(current_pose)

        if distance >= 0.5:
            # 如果符合条件，那么将当前位姿赋值给last_pose，并添加进列表
            self.last_pose = current_pose
            self.append_last_pose()
        # 发布
        self.publish_path()

def main():
    # 初始化ROS2客户端
    rclpy.init()
    # 调用spin函数，并传入节点对象
    rclpy.spin(Go2OdomPath())
    # 资源释放
    rclpy.shutdown()

if __name__ == '__main__':
    main()